/*
 * Author: Franz Pucher
 */

#ifndef DIFFBOT_BASE_CONTROLLER_H
#define DIFFBOT_BASE_CONTROLLER_H


#include <ros.h>
#include <diffbot_msgs/EncodersStamped.h>
#include <diffbot_msgs/WheelsCmdStamped.h>
#include <diffbot_msgs/AngularVelocities.h>
#include <diffbot_msgs/PIDStamped.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/JointState.h>

#include "diffbot_base_config.h"
#include "encoder_diffbot.h"
#include "adafruit_feather_wing/adafruit_feather_wing.h"
#include "pid.h"


namespace diffbot {

    /** 
     * \brief 与高层硬件接口 `hardware_interface::RobotHW` 进行通信，并与机器人的硬件传感器（如编码器）和执行器（如电机驱动器）进行交互。
     * 
     * BaseController 类通过 ROS 的发布器和订阅器与高层接口 `diffbot_base::hardware_interface::RobotHW` 进行通信。
     * 在主循环中（参见 `main.cpp`），该类以特定的更新频率 `update_rate_` 操作不同的传感器和执行器。
     * 存在三种更新频率：
     * - control（控制频率）
     * - imu（IMU 频率）
     * - debug（调试频率）
     * 
     * 控制更新频率用于从编码器读取数据，并以一定的速率向电机写入计算得到的 pwm 信号。
     * 这是为了避免因为运行的计算过多而导致同步错误。
     * 类似地，IMU 更新频率则用于以特定速率读取最新的惯性测量数据。
     * 为了追踪何时需要更新，使用了成员变量 `last_update_time_` 来存储上次更新的时间戳。
     * 另外，`LastUpdateTime::command_received` 时间戳用于检查是否与高层硬件接口失去连接，如果在指定时间内未接收到命令消息，将停止电机（参见 `eStop`）。
     * 
     * BaseController 订阅目标轮子命令的角速度（`diffbot_msgs::WheelsCmdStamped` 消息，订阅 `wheel_cmd_velocities` 话题），
     * 使用 `sub_wheel_cmd_velocities_` 订阅并保持指向左右电机控制器的指针，分别是 `p_motor_controller_right_` 和 `p_motor_controller_left_`，这些指针是通过通用的电机驱动器无关接口 `diffbot::MotorControllerIntf` 获取的。
     * 每次接收到新的 `diffbot_msgs::WheelsCmdStamped` 消息时，`commandCallback` 方法会被调用，并更新左右电机的目标角速度 `wheel_cmd_velocity_left_` 和 `wheel_cmd_velocity_right_`。
     * 
     * 为了测量角轮位置（绝对位置）和角速度，使用了两个 `diffbot::Encoder` 对象（`encoder_left_` 和 `encoder_right_`）。
     * 这些对象会读取 `diffbot::JointStates`，并将其存储在 `joint_state_left_` 和 `joint_state_right_` 中。
     * 在读取到最新的状态（位置和速度）后，通过 `pub_measured_joint_states_` 发布到 `measured_joint_states` 话题，消息类型为 `sensor_msgs::JointState`。
     * `diffbot_base::hardware_interface::RobotHW` 会订阅这些关节状态并将其传递给 `diff_drive_controller`。
     * 同时，在 `read()` 方法内部，会读取来自 `encoder_left_` 和 `encoder_right_` 的当前编码器值，并将其存储在 `ticks_left_` 和 `ticks_right_` 中。
     * 
     * 测得的角速度对于计算每个电机的 pwm 信号非常重要，计算时使用了两个独立的 PID 控制器（`motor_pid_left_` 和 `motor_pid_right_`），
     * 它们根据测得的角速度与命令的角速度之间的误差来计算。
     * 
     * 在初始化时，从 ROS 参数服务器中读取以下参数：
     * - /diffbot/encoder_resolution
     * - /diffbot/mobile_base_controller/wheel_radius
     * - /diffbot/mobile_base_controller/linear/x/max_velocity
     * - /diffbot/debug/base_controller
     * 
     * @tparam TMotorController 电机控制器类型
     * @tparam TMotorDriver 电机驱动器类型
     */
    template <typename TMotorController, typename TMotorDriver>
    class BaseController
    {
    public:

        /**
         * @brief 使用两个通用电机控制器构造 Base Controller 对象
         * 
         * 需要两个已初始化的通用电机控制器，这些控制器在父级作用域中定义（例如 `main.cpp`）。
         * 左右电机的两个控制器保持通用，唯一的要求是实现 \ref diffbot::MotorControllerIntf，
         * 该接口包含电机驱动器，因此具有控制单个电机的能力。
         * 
         * @param nh 全局 ROS 节点句柄的引用
         * @param motor_controller_left 指向左电机的通用电机控制器的指针
         * @param motor_controller_right 指向右电机的通用电机控制器的指针
         */
        BaseController(ros::NodeHandle &nh, TMotorController* motor_controller_left, TMotorController* motor_controller_right);

        /**
         * @brief 存储主控制循环、（可选）IMU 和调试输出的更新率频率（Hz）。
         * 
         * UpdateRate 包含一个嵌套结构 \ref period_ T，用于定义频率的倒数
         * T = 1/f Hz，单位为秒。这个周期（间接地是频率）与 \ref last_update_time_ 一起使用，
         * 用于比较是否已经经过足够的时间来进行新的更新。这个比较是在主循环中进行的（见 main.cpp）。
         */
        struct UpdateRate
        {
            double control_;
            double debug_;

            struct Period
            {
                double control_;
                double debug_;

                inline Period(double control_frequency, double debug_frequency)
                    : control_(1.0 / control_frequency)
                    , debug_(1.0 / debug_frequency) {};
            } period_;

            inline Period& period() { return period_; };

            inline UpdateRate(double control_frequency,
                             double debug_frequency)
                : control_(control_frequency)
                , debug_(debug_frequency)
                , period_(control_frequency, debug_frequency) {};
        } update_rate_;

        /**
         * @brief 根据给定的频率 \p frequency (Hz) 计算周期 (s)。
         * 
         * @param frequency 输入频率 (Hz)，将被转换为周期。
         * @return int 以秒为单位的周期。
         */
        inline int period(double frequency) { return 1 / frequency; };

        inline UpdateRate& publishRate() { return update_rate_; };

        /**
         * @brief 跟踪最后更新时间。
         * 
         * 在 main.cpp 中，每当相应事件发生时，\ref last_update_time_ 的成员就会被更新。
         * 这与 \ref update_rate_ 一起使用，用于检查是否已经过了足够的时间来进行新的更新。
         * 例如读取编码器的新值和写入接收到的电机命令。
         */
        struct LastUpdateTime
        {
            ros::Time command_received;
            ros::Time control;
            ros::Time debug;

            inline LastUpdateTime(ros::Time start)
                : command_received(start.toSec(), start.toNsec())
                , control(start.toSec(), start.toNsec())
                , debug(start.toSec(), start.toNsec()) {};
        } last_update_time_;

        /**
         * @brief 返回对 \ref last_update_times_ 的引用。
         * 
         * 在 main.cpp 的主循环中使用，用于将当前时间与特定功能
         * (command_received, control, imu, debug) 的上次更新时间进行比较，
         * 以确定是否需要更新。
         * 
         * @return LastUpdateTime& 上一次更新时间。
         */
        inline LastUpdateTime& lastUpdateTime() { return last_update_time_; };


        /**
         * @brief 获取固件是否应该记录调试输出的状态。
         * 
         * @return true 
         * @return false 
         */
        inline bool debug() { return debug_; };

        /**
         * @brief 初始化主节点句柄并设置发布者和订阅者。
         * 
         * 等待直到与 ROS master 的连接建立。
         */
        void setup();

        /**
         * @brief 从参数服务器读取参数
         * 
         * - 从参数服务器获取参数
         *   - /diffbot/encoder_resolution
         *   - /diffbot/mobile_base_controller/wheel_radius
         *   - /diffbot/mobile_base_controller/linear/x/max_velocity
         *   - /diffbot/debug/base_controller
         * - 初始化 DiffBot 轮式编码器
         * - 将两个轮式编码器的计数重置为零
         * - 根据读取的 \ref max_linear_velocity_ 和 \ref wheel_radius_ 初始化 \ref max_angular_velocity_
         */
        void init();

        /**
         * @brief 当长时间未收到轮子命令时停止电机。
         * 
         * 将 \ref wheel_cmd_velocity_left_ 和 \ref wheel_cmd_velocity_right_ 设置为零。
         * 当在 /diffbot/wheel_cmd_velocities 主题上的 \ref commandCallback 
         * 在 \ref LastUpdateTime::command_received 周期内未被调用时，将调用此方法。
         * 
         */
        void eStop();

        /**
         * @brief 读取当前编码器计数和关节状态（角位置(rad)和角速度(rad/s)）
         *        从左 \ref encoder_left_ 和右 \ref encoder_right_ 两个编码器，
         *        并使用 \ref pub_measured_joint_states_ 在 "measured_joint_states" 主题上
         *        发布 sensor_msgs::JointState
         *
         */
        void read();

        /**
         * @brief 使用 PID 计算左右电机的受限 PWM 信号。
         * 
         * 发送到两个电机驱动器的值由两个 PID \ref pid_motor_left_ 和 \ref pid_motor_right_ 
         * 根据命令角速度与测量角速度之间的误差计算
         * (例如 diffbot::PID::error_ = \ref wheel_cmd_velocity_left_ - \ref measured_angular_velocity_left_)
         * 
         * 计算出的 PID 输出值在 -/+ MAX_RPM 处受限，以防止 PID 产生过大的误差
         * 计算出的受限 PID 值然后使用 \ref diffbot::MotorControllerIntf::setSpeed 方法
         * 设置给左右电机 \ref p_motor_controller_left_ 和 \ref p_motor_controller_right_。
         * 
         */
        void write();
        void printDebug();

        /**
         * @brief 当在 wheel_cmd_velocities 主题上收到新的 \ref diffbot_msgs::WheelsCmdStamped 时的回调方法。
         * 
         * 每当从 'wheel_cmd_velocities' 主题接收到每个轮子关节的角速度命令时的回调方法。
         * 该主题由高级 \ref hardware_interface::RobotHW::write() 方法发布。
         * 
         * 此回调方法接收 \ref diffbot_msgs::WheelsCmdStamped 消息对象 (\p cmd_msg)，
         * 其中包含 \ref diffbot_msgs::AngularVelocities，存储了左右轮子的命令关节值。
         * 接收到消息后，会更新 \ref wheel_cmd_velocity_left_ 和 \ref wheel_cmd_velocity_right_ 成员，
         * 这些值将在控制循环的下一次 \ref write() 中使用。
         * 
         * \code
         * wheel_cmd_velocity_left_ = cmd_msg.wheels_cmd.angular_velocities.joint[0];
         * wheel_cmd_velocity_right_ = cmd_msg.wheels_cmd.angular_velocities.joint[1];
         * \endcode
         * 
         * 在此方法中，\ref lastUpdateTime::command_received 时间戳被设置为当前时间。
         * 这用于 eStop 功能。如果在 wheel_cmd_velocities 主题上没有收到 
         * diffbot_msgs::WheelsCmdStamped 消息，则会调用 eStop 方法（参见 main.cpp 中的主循环）
         * 
         * @param cmd_msg 包含命令轮速的消息。
         */
        void commandCallback(const diffbot_msgs::WheelsCmdStamped& cmd_msg);

        /**
         * @brief 将两个编码器的计数重置为零的回调方法。
         * 
         * 为了初始化 BaseController，编码器计数被设置回零。
         * 每次启动 diffbot_bringup/launch/bringup.launch 时，高级
         * hardware_interface::RobotHW 都会在 /reset 主题上发布一个空消息，
         * 这会触发此回调并将编码器设置为零。
         * 
         * @param reset_msg 空消息（未使用）
         */
        void resetEncodersCallback(const std_msgs::Empty& reset_msg);

        /**
         * @brief 更新左电机 PID 常数的回调方法。
         *
         * 发布到 pid_left 主题以更新左电机的 PID 常数。
         *
         * @param pid_msg 包含新 PID 常数的消息
         */
        void pidLeftCallback(const diffbot_msgs::PIDStamped& pid_msg);

        /**
         * @brief 更新右电机 PID 常数的回调方法。
         *
         * 发布到 pid_right 主题以更新右电机的 PID 常数。
         *
         * @param pid_msg 包含新 PID 常数的消息
         */
        void pidRightCallback(const diffbot_msgs::PIDStamped& pid_msg);

    private:
        // 来自 main.cpp 的全局节点句柄引用
        ros::NodeHandle& nh_;

        // 常量
        float wheel_radius_ = 0.0;
        float max_linear_velocity_ = 0.0;
        float max_angular_velocity_ = 0.0;

        // 编码器设置
        // 将这些引脚号更改为连接到编码器的引脚。
        //   最佳性能：两个引脚都具有中断功能
        //   良好性能：只有第一个引脚具有中断功能
        //   低性能：两个引脚都没有中断功能
        // 避免使用带有 LED 的引脚
        diffbot::Encoder encoder_left_;
        diffbot::Encoder encoder_right_;
        long ticks_left_ = 0, ticks_right_ = 0;

        // 测量的左右关节状态（角位置(rad)和角速度(rad/s)）
        diffbot::JointState joint_state_left_, joint_state_right_;

        int encoder_resolution_;

        ros::Subscriber<std_msgs::Empty, BaseController<TMotorController, TMotorDriver>> sub_reset_encoders_;

        // ROS 发布者设置，用于发布左右编码器计数
        // 这使用自定义编码器计数消息，定义了一个包含两个整数的数组
        diffbot_msgs::EncodersStamped encoder_msg_;
        ros::Publisher pub_encoders_;

        sensor_msgs::JointState msg_measured_joint_states_;
        ros::Publisher pub_measured_joint_states_;

        MotorControllerIntf<TMotorDriver>* p_motor_controller_right_;
        MotorControllerIntf<TMotorDriver>* p_motor_controller_left_;

        ros::Subscriber<diffbot_msgs::WheelsCmdStamped, BaseController<TMotorController, TMotorDriver>> sub_wheel_cmd_velocities_;
        float wheel_cmd_velocity_left_ = 0.0;
        float wheel_cmd_velocity_right_ = 0.0;

        int motor_cmd_left_ = 0;
        int motor_cmd_right_ = 0;

        ros::Subscriber<diffbot_msgs::PIDStamped, BaseController<TMotorController, TMotorDriver>> sub_pid_left_;
        ros::Subscriber<diffbot_msgs::PIDStamped, BaseController<TMotorController, TMotorDriver>> sub_pid_right_;
        PID motor_pid_left_;
        PID motor_pid_right_;

        // 调试
        bool debug_;
    };

}

template <typename TMotorController, typename TMotorDriver>
using BC = diffbot::BaseController<TMotorController, TMotorDriver>;


template <typename TMotorController, typename TMotorDriver>
diffbot::BaseController<TMotorController, TMotorDriver>
    ::BaseController(ros::NodeHandle &nh, TMotorController* motor_controller_left, TMotorController* motor_controller_right)
    : nh_(nh)
    , encoder_left_(nh, ENCODER_LEFT_H1, ENCODER_LEFT_H2, ENCODER_RESOLUTION)
    , encoder_right_(nh, ENCODER_RIGHT_H1, ENCODER_RIGHT_H2, ENCODER_RESOLUTION)
    , sub_reset_encoders_("reset", &BC<TMotorController, TMotorDriver>::resetEncodersCallback, this)
    , pub_encoders_("encoder_ticks", &encoder_msg_)
    , pub_measured_joint_states_("measured_joint_states", &msg_measured_joint_states_)
    , sub_wheel_cmd_velocities_("wheel_cmd_velocities", &BC<TMotorController, TMotorDriver>::commandCallback, this)
    , last_update_time_(nh.now())
    , update_rate_(UPDATE_RATE_CONTROL, UPDATE_RATE_DEBUG)
    , sub_pid_left_("pid_left", &BC<TMotorController, TMotorDriver>::pidLeftCallback, this)
    , sub_pid_right_("pid_right", &BC<TMotorController, TMotorDriver>::pidRightCallback, this)
    , motor_pid_left_(PWM_MIN, PWM_MAX, K_P, K_I, K_D)
    , motor_pid_right_(PWM_MIN, PWM_MAX, K_P, K_I, K_D)
{
    p_motor_controller_left_ = motor_controller_left;
    p_motor_controller_right_ = motor_controller_right;
}


template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::setup()
{
    nh_.initNode();
    nh_.advertise(pub_encoders_);

    // msg_measured_joint_states_ 的类型是 sensor_msgs::JointState
    // 它包含未定义大小的 float[] joint 数组。
    // 为了使 rosserial 工作，需要使用 malloc 保留内存
    // 并适当设置 *_length 成员。
    // http://wiki.ros.org/rosserial/Overview/Limitations#Arrays
    msg_measured_joint_states_.position = (float*)malloc(sizeof(float) * 2);
    msg_measured_joint_states_.position_length = 2;
    msg_measured_joint_states_.velocity = (float*)malloc(sizeof(float) * 2);
    msg_measured_joint_states_.velocity_length = 2;
    nh_.advertise(pub_measured_joint_states_);

    nh_.subscribe(sub_wheel_cmd_velocities_);
    nh_.subscribe(sub_reset_encoders_);

    nh_.subscribe(sub_pid_left_);
    nh_.subscribe(sub_pid_right_);

    while (!nh_.connected())
    {
        nh_.spinOnce();
    }
}

template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::init()
{
    nh_.loginfo("从参数服务器获取参数");
    nh_.getParam("/diffbot/encoder_resolution", &this->encoder_resolution_);
    String log_msg = String("/diffbot/encoder_resolution: ") + String(encoder_resolution_);
    nh_.loginfo(log_msg.c_str());
    nh_.getParam("/diffbot/mobile_base_controller/wheel_radius", &wheel_radius_);
    log_msg = String("/diffbot/mobile_base_controller/wheel_radius: ") + String(wheel_radius_);
    nh_.loginfo(log_msg.c_str());
    nh_.getParam("/diffbot/mobile_base_controller/linear/x/max_velocity", &max_linear_velocity_);
    log_msg = String("/diffbot/mobile_base_controller/linear/x/max_velocity: ") + String(max_linear_velocity_);
    nh_.loginfo(log_msg.c_str());
    nh_.getParam("/diffbot/debug/base_controller", &debug_);
    log_msg = String("/diffbot/debug/base_controller: ") + String(debug_);
    nh_.loginfo(log_msg.c_str());

    nh_.loginfo("初始化 DiffBot 轮式编码器");
    encoder_left_.resolution(encoder_resolution_);
    encoder_right_.resolution(encoder_resolution_);

    std_msgs::Empty reset;
    this->resetEncodersCallback(reset);
    delay(1);

    max_angular_velocity_ = max_linear_velocity_ / wheel_radius_;

    delay(1000);
}

template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::commandCallback(const diffbot_msgs::WheelsCmdStamped& cmd_msg)
{
    // 每次从 'wheel_cmd_velocities' 主题接收到每个轮子关节的角速度命令时的回调函数
    // 此回调函数接收 diffbot_msgs::WheelsCmdStamped 消息对象
    // 其中存储了两个关节的 diffbot_msgs::AngularVelocities
    wheel_cmd_velocity_left_ = cmd_msg.wheels_cmd.angular_velocities.joint[0];
    wheel_cmd_velocity_right_ = cmd_msg.wheels_cmd.angular_velocities.joint[1];

    // 用于 eStop。如果在 wheel_cmd_velocities 主题上没有收到 diffbot_msgs::WheelsCmdStamped 消息，
    // 则会调用 eStop 方法（参见 main.cpp 中的主循环）
    lastUpdateTime().command_received = nh_.now();
}

// ROS 订阅者设置，用于将两个编码器重置为零
template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::resetEncodersCallback(const std_msgs::Empty& reset_msg)
{
    // 将两个编码器重置回零。
    this->encoder_left_.write(0);
    this->encoder_right_.write(0);
    this->nh_.loginfo("将两个轮式编码器重置为零");
}


template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::pidLeftCallback(const diffbot_msgs::PIDStamped& pid_msg)
{
    // 更新左电机 pid 值的回调函数
    // 此回调函数接收 diffbot_msgs::PID 消息对象
    // 其中存储了一个 pid 控制器的 diffbot_msgs::PID kp, ki, kd
    motor_pid_left_.updateConstants(pid_msg.pid.kp, pid_msg.pid.ki, pid_msg.pid.kd);
    motor_pid_left_.updateConstants(pid_msg.pid.kp, pid_msg.pid.ki, pid_msg.pid.kd);
}

template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::pidRightCallback(const diffbot_msgs::PIDStamped& pid_msg)
{
    // 更新左电机 pid 值的回调函数
    // 此回调函数接收 diffbot_msgs::PID 消息对象
    // 其中存储了一个 pid 控制器的 diffbot_msgs::PID kp, ki, kd
    motor_pid_left_.updateConstants(pid_msg.pid.kp, pid_msg.pid.ki, pid_msg.pid.kd);
    motor_pid_left_.updateConstants(pid_msg.pid.kp, pid_msg.pid.ki, pid_msg.pid.kd);
}


template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::read()
{
    joint_state_left_ = encoder_left_.jointState();
    joint_state_right_ = encoder_right_.jointState();

    msg_measured_joint_states_.position[0] = joint_state_left_.angular_position_;
    msg_measured_joint_states_.position[1] = joint_state_right_.angular_position_;

    msg_measured_joint_states_.velocity[0] = joint_state_left_.angular_velocity_;
    msg_measured_joint_states_.velocity[1] = joint_state_right_.angular_velocity_;
    pub_measured_joint_states_.publish(&msg_measured_joint_states_);

    // 获取每个编码器的当前计数
    ticks_left_ = encoder_left_.read();
    ticks_right_ = encoder_right_.read();

    encoder_msg_.encoders.ticks[0] = ticks_left_;
    encoder_msg_.encoders.ticks[1] = ticks_right_;
    // 避免有太多发布者
    // 否则会出现 'wrong checksum for topic id and msg'
    // 和 'Write timeout: Write timeout' 等错误。
    //pub_encoders_.publish(&encoder_msg_);
}

template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::write()
{
    // https://www.arduino.cc/reference/en/language/functions/math/map/
    // map(value, fromLow, fromHigh, toLow, toHigh)
    // 将角速度映射到电机命令
    // 从 ROS 参数服务器获取的 /diffbot/mobile_base_controller/linear/x/max_velocity
    // 被转换为最大旋转速度（见 init()）并用于将角速度映射到电机的 PWM 信号。
    // 注意，这目前未使用，因为 PID 有助于保持速度与命令一致。
    //motor_cmd_left_ = map(wheel_cmd_velocity_left_, -max_angular_velocity_, max_angular_velocity_, PWM_MIN, PWM_MAX);
    //motor_cmd_right_ = map(wheel_cmd_velocity_right_, -max_angular_velocity_, max_angular_velocity_, PWM_MIN, PWM_MAX);

    // 计算 PID 输出
    // 发送到电机驱动器的值由 PID 根据命令角速度与测量角速度之间的误差计算
    // 计算出的 PID 输出值在 -/+ MAX_RPM 处受限，以防止 PID 产生过大的误差
    motor_cmd_left_ = motor_pid_left_.compute(wheel_cmd_velocity_left_, joint_state_left_.angular_velocity_);
    motor_cmd_right_ = motor_pid_right_.compute(wheel_cmd_velocity_right_, joint_state_right_.angular_velocity_);

    p_motor_controller_left_->setSpeed(motor_cmd_left_);
    p_motor_controller_right_->setSpeed(motor_cmd_right_);
}

template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::eStop()
{
    wheel_cmd_velocity_left_ = 0;
    wheel_cmd_velocity_right_ = 0;
}

template <typename TMotorController, typename TMotorDriver>
void diffbot::BaseController<TMotorController, TMotorDriver>::printDebug()
{
    /*
    String log_msg =
            String("\nRead:\n") +
                String("ticks_left_ \t ticks_right_ \t measured_ang_vel_left \t measured_ang_vel_right\n") +
                String(ticks_left_) + String("\t") + String(ticks_right_) + String("\t") +
                String(joint_state_left_.angular_velocity_) + String("\t") + String(joint_state_right_.angular_velocity_) +
            String("\nWrite:\n") + 
                String("motor_cmd_left_ \t motor_cmd_right_ \t pid_left_error \t pid_right_error\n") +
                String(motor_cmd_left_) + String("\t") + String(motor_cmd_right_) + String("\t") +
                //String("pid_left \t pid_right\n") +
                String(motor_pid_left_.error()) + String("\t") + String(motor_pid_right_.error());
    */

    String log_msg =
            String("\nRead:\n") +
                String("ticks_left_: ") + String(ticks_left_) +
                String("\nticks_right_: ") + String(ticks_right_) +
                String("\nmeasured_ang_vel_left: ") + String(joint_state_left_.angular_velocity_) +
                String("\nmeasured_ang_vel_right: ") + String(joint_state_right_.angular_velocity_) +
                String("\nwheel_cmd_velocity_left_: ") + String(wheel_cmd_velocity_left_) +
                String("\nwheel_cmd_velocity_right_: ") + String(wheel_cmd_velocity_right_) +

            String("\nWrite:\n") +
                String("motor_cmd_left_: ") + String(motor_cmd_left_) +
                String("\nmotor_cmd_right_: ") + String(motor_cmd_right_) +
                String("\npid_left_errors (p, i, d): ") + String(motor_pid_left_.proportional()) + String(" ") + String(motor_pid_left_.integral()) + String(" ") + String(motor_pid_left_.derivative()) +
                String("\npid_right_error (p, i, d): ") + String(motor_pid_right_.proportional()) + String(" ") + String(motor_pid_right_.integral()) + String(" ") + String(motor_pid_right_.derivative());
    nh_.loginfo(log_msg.c_str());
}


#endif // DIFFBOT_BASE_CONTROLLER_H